#include <cstdlib>
#include<stdlib.h>
#include <iostream>
#include <stdio.h>
#include <time.h>
#include <pthread.h>
#include <sys/siginfo.h>
#include <sys/neutrino.h>
#include <signal.h>
#include <errno.h>
#include <unistd.h>

/* 
 * COEN 320 Assignment 1 Code
 * Concordia University
 * 
 * By: 
 * Alex Ionita Teodor	(ID: 9154558)
 * Matthias Martineau	(ID: 1959696)
 * 
 * Due: October 25 2013
 */

class Timer
{
public:
	int channel_id;
	
	Timer(){
		if ((channel_id = ChannelCreate (0)) == -1) {
			perror (NULL);
			return;
		}
	}
	void wait()
	{
		MsgReceive (channel_id, NULL, NULL, NULL);
	
	}
	
	void setup(unsigned int seconds)
	{
		timer_t    timerid;    
		sigevent   event;     
	    itimerspec timer;  
	    int        coid;     
	     
		// connection back to the previous channel created
	    coid = ConnectAttach (0, 0, channel_id, 0, 0);
	     
	    if (coid == -1) {
	    	printf("Error coid");
	    	perror (NULL);
	    	return;
	    	//return 0;
	    }
		     
		// macro to create a pulse event
		SIGEV_PULSE_INIT (&event, coid, SIGEV_PULSE_PRIO_INHERIT, 1, 0);
		  
		//creating the timer object
		if (timer_create (CLOCK_REALTIME, &event, &timerid) == -1) {
		    perror(NULL);
		    printf("error"); 
		    return;
		    //return(0);
		}
	     
		// configuring the timer with the arguments
		timer.it_value.tv_sec = seconds;
		timer.it_value.tv_nsec = 0;
		timer.it_interval.tv_sec = seconds;
		timer.it_interval.tv_nsec = 0;

		// finally, we start it
		timer_settime (timerid, 0, &timer, NULL);
	}
};

struct GlobalTimer{
	double DD_total_time;
	double DAP_total_time;
};

struct PosVel
{
	double Vx;
	double Vy;
	double Vz;
	double X;
	double Y;
	double Z;
};


//Global variables

float Ax = -2;
float Ay = 4.0;
float Az = 10.5;

int channel_id_DD;  // channel ID
int channel_id_DAP; // channel ID

int DAPupdate = 2;
int DDupdate = 6;

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

PosVel posVel;
GlobalTimer globalTimer;

double milliSecDiff(timespec start, timespec end)
{
	double timeDiff = 0.0;
	double secDiff = (double) (end.tv_sec - start.tv_sec);
	double milliSecDiff = ( (double) (end.tv_nsec - start.tv_nsec) / (double)1000000000L);
	
	timeDiff = secDiff + milliSecDiff;
	
	return timeDiff;

}

void displayData()
{
//	printf("\n");
	printf("Vx: %f",posVel.Vx);
	printf("\tVy: %f",posVel.Vy);
	printf("\tVz: %f\n",posVel.Vz);
	printf("X: %f", posVel.X);
	printf("\tY: %f", posVel.Y);
	printf("\tZ: %f\n\n", posVel.Z);

}

int dummyCalculations(int val){
	for (int i = 1; i < 999; i++){
			for (int j = 1; j < 999; j++){
			val = i / j;
			}
		}
	return val;
}

void updateData()
{
	posVel.Vx += Ax * DAPupdate;
	posVel.Vy += Ay * DAPupdate;
	posVel.Vz += (Az - 9.81) * DAPupdate;
	posVel.X  += posVel.Vx * DAPupdate; 
	posVel.Y  += posVel.Vy * DAPupdate;
	posVel.Z  += posVel.Vz * DAPupdate;
	
	//Simulate calculation time
	unsigned int k = 0;
	k = dummyCalculations(k);
}

void *DAP(void *)
{
	Timer timer;
	timer.setup(DAPupdate);
	
    timespec time_pre;
    timespec time_post;
    double milli_sec_diff = 0;
	
	printf("DAP thread started\n");
	
	while(1){
					
		//Lock mutex
		pthread_mutex_lock( &mutex );
		
		
		clock_gettime(CLOCK_REALTIME, &time_pre);
		
		
		//Display data
		updateData();
		
		clock_gettime( CLOCK_REALTIME, &time_post);
		
		//Release mutex
		pthread_mutex_unlock( &mutex );
				
		//Calculate execution time and add it to total DAP time so far
		milli_sec_diff = milliSecDiff(time_pre, time_post);
		globalTimer.DAP_total_time += milli_sec_diff;
	    
		
		
		//compensating for system call time
	    clock_gettime( CLOCK_REALTIME, &time_pre);
	    clock_gettime( CLOCK_REALTIME, &time_post);
	    milli_sec_diff = milliSecDiff(time_pre, time_post);
	    globalTimer.DAP_total_time -= (2 * milli_sec_diff);
		    

		//block until pulse
		timer.wait();
	};
	
	return NULL;
}

void *DD(void *)
{
	Timer timer;
    timespec time_pre;
    timespec time_post;
    double milli_sec_diff;

	timer.setup(DDupdate);
	printf("DD thread started\n");
	while(1){
		
			
		//lock mutex
		pthread_mutex_lock( &mutex );
		
		
		clock_gettime( CLOCK_REALTIME, &time_pre);
		//display data
		displayData();
		//release mutex
		clock_gettime( CLOCK_REALTIME, &time_post);
		
		
		pthread_mutex_unlock( &mutex );
		
	    

		//Calculate execution time and add it to total DAP time so far
	    milli_sec_diff = milliSecDiff(time_pre, time_post);
	    
	    globalTimer.DD_total_time += milli_sec_diff;

	    
	    //compensating for system call time
	    clock_gettime( CLOCK_REALTIME, &time_pre);
	    clock_gettime( CLOCK_REALTIME, &time_pre);
	    milli_sec_diff = milliSecDiff(time_pre, time_post);
	    globalTimer.DD_total_time -= (2 * milli_sec_diff);
	    
		//block until pulse
	    timer.wait();
	};
	return NULL;
}

int main(int argc, char *argv[]) {
	//http://www.qnx.com/developers/docs/6.4.1/neutrino/getting_started/s1_timer.html#using
	
	//Initialize structs
	posVel.X  = 0;
	posVel.Y  = 0;
	posVel.Z  = 0;
	posVel.Vx = 0;
	posVel.Vy = 0;
	posVel.Vz = 0;
	
	globalTimer.DAP_total_time = 0;
	globalTimer.DD_total_time = 0;
	
	//Thread Initialization
	pthread_t DAP_thread, DD_thread;

	pthread_create(&DAP_thread, NULL, DAP, NULL);
	pthread_create(&DD_thread, NULL, DD, NULL);

	//Let the threads run for 30 seconds
	sleep(30);
	
	//Kill threads
	pthread_detach(DAP_thread);
	pthread_detach(DD_thread);
	
	//Display results
	printf("Complete!\nTotal DAP Time: %f\tTotal DD Time: %f", globalTimer.DAP_total_time, globalTimer.DD_total_time);

	return EXIT_SUCCESS;
}

